import * as THREE from "../../../modules/three/three.module.js";
import {SprayDroneCollide} from "../lib/CommonConsts.js";
import {OBB} from "../../../modules/three/jsm/math/OBB.js";
import {BoolPolygonsUtil, isBetween} from "../lib/util/bool-polygons-util.js";
import {findConvexHull} from "../lib/util/StaticMethods.js";
import BlockingMesh from "../lib/gadget/BlockingMesh.js";
import {segmentsFromCorners, twoBoxesIntersected, twoObbsIntersected} from "../lib/util/QuadPair.js";
import ClipperTool from "../lib/clipper_js/ClipperTool.js";

// true 表示：计算碰撞时，不使用 定向包围盒obb，而是使用obb 对应的包围盒Box3，
// 好处是提升性能，可以极大的减少搜索时需要遍历的空间坐标节点，坏处是找到的多数航线都有点绕远，航线偏长
const onlyBox3 = true;
const config = SprayDroneCollide;
const mat = new THREE.Matrix4();

const planeDown = new THREE.Plane(new THREE.Vector3(0,0,1));
const planeUp = new THREE.Plane(new THREE.Vector3(0,0,1), -1);
const tmp = new THREE.Vector3();
const tmp2 = new THREE.Vector3();
const xAxis = new THREE.Vector3(1, 0, 0);
const yAxis = new THREE.Vector3(0, 1, 0);
const zAxis = new THREE.Vector3(0, 0, 1);
const xNegAxis = new THREE.Vector3(-1, 0, 0);
const yNegAxis = new THREE.Vector3(0, -1, 0);
const zNegAxis = new THREE.Vector3(0, 0, -1);
const axes = [xAxis, yAxis, zAxis, xNegAxis, yNegAxis, zNegAxis];
const axesStr = "+x, +y, +z, -x, -y, -z".split(', ');
const axesField = "x, y, z, x, y, z".split(', ');
const axisToStr = new Map();
const axisToField = new Map();
for (let i = 0; i < axes.length; i ++) {
  axisToStr.set(axes[i], axesStr[i]);
  axisToField.set(axes[i], axesField[i]);
}

const corners = [];
for (let i = 0; i < 8; i ++) {
  corners.push(new THREE.Vector3());
}

const epsilon = 0.01;
const obbB_indicator = new BlockingMesh();
obbB_indicator.material = new THREE.MeshBasicMaterial({
  color: '#b21313',
  transparent: true,
  opacity: 0.3,
});

/**
 * @param {WorkArea[]} workAreas
 * @param {WorkArea} area
 */
function selectMaxZArea(workAreas, area) {
  const sameOriented = workAreas.filter(it => it.facing.dot(area.facing) > 0.98);
  const zVals = [];
  sameOriented.forEach(workArea => {
    const pos = new THREE.Vector3().setFromMatrixPosition(workArea.planeMat);
    pos.applyMatrix4(area.planeMatInv);
    zVals.push(pos.z);
  });
  const maxZ = Math.max(...zVals);
  return sameOriented[zVals.indexOf(maxZ)];
}


/**
 * @param {Blocking} blocking
 * @return {OBB}
 */
function obbFromBlocking(blocking) {
  const obb = new OBB();
  obb.center.copy(blocking.position);
  obb.halfSize.copy(blocking.scale).multiplyScalar(0.5);
  mat.makeRotationFromEuler(blocking.rotation);
  obb.rotation.setFromMatrix4(mat);
  return obb;
}

/**
 * 返回工作区本地坐标系中的包围盒
 * @param {WorkArea} workArea
 * @return {THREE.Box3[]}
 */
function boxArrFromWorkArea(workArea) {
  const gunLen = config.gunCollideLength;
  workArea.updateLocalBox();
  const {min, max} = workArea.localBox3;
  const min0 = min.clone();
  const max0 = max.clone();
  min0.x -= config.gunCollideRadius;
  min0.y -= config.gunCollideRadius;
  max0.x += config.gunCollideRadius;
  max0.y += config.gunCollideRadius;
  min0.z = -gunLen;
  const box0 = new THREE.Box3(min0, max0);

  const min1 = min.clone();
  const max1 = max.clone();
  min1.x -= config.expandH;
  max1.x += config.expandH;
  min1.y -= config.expandVerticalDown;
  max1.y += config.expandVerticalUp;
  // 这里表明，喷枪活动体积空间 和 无人机机身活动空间，这个两个包围盒有重叠
  min1.z = -(gunLen + config.droneDeep + config.gapBack);
  max1.z = -gunLen + config.gapFront;
  const box1 = new THREE.Box3(min1, max1);
  return [box0, box1];
}

/**
 * 上下左右前后，六个方向，来扩展包围盒；
 * @param {THREE.Box3} box0，包围盒子
 * @param {THREE.Vector3} direction，direction 上下左右前后，六个方向
 * @param {number} movement，沿direction 方向移动的距离，必须为正数
 * @return {THREE.Box3}
 */
function box3ExpandByMovement(box0, direction, movement) {
  const box = new THREE.Box3().copy(box0);
  if (direction.equals(xAxis)) {
    box.max.x += movement;
  } else if (direction.equals(xNegAxis)) {
    box.min.x -= movement;
  } else if (direction.equals(yAxis)) {
    box.max.y += movement;
  } else if (direction.equals(yNegAxis)) {
    box.min.y -= movement;
  } else if (direction.equals(zAxis)) {
    box.max.z += movement;
  } else {
    box.min.z -= movement;
  }
  return box;
}

/**
 * 根据无人机的活动方向，求无人机的活动体积；
 * @param {THREE.Vector3} direction，direction 上下左右前后，六个方向
 * @param {number} movement，沿direction 方向移动的距离，必须为正数
 * @return {THREE.Box3[]} 位于 以喷嘴为原点的坐标系，该坐标系的 x轴 y轴 z轴 朝向和 工作区的本地坐标系一致
 */
export function boxArrFromMovementOfDrone(direction, movement) {
  const r = config.gunCollideRadius;
  const hw = config.bodyCollideWidth * 0.5;
  const hh = config.bodyCollideHeight * 0.5;
  const d0 = -(config.gunCollideLength + config.droneDeep + config.gapBack);
  const d1 = -config.gunCollideLength + config.gapFront;
  let boxGun = new THREE.Box3(new THREE.Vector3(-r, -r, -config.gunCollideLength), new THREE.Vector3(r, r, 0));
  let boxBody = new THREE.Box3(new THREE.Vector3(-hw, -hh, d0), new THREE.Vector3(hw, hh, d1));
  boxBody.translate(new THREE.Vector3(0, config.centerToGun, 0));

  if (direction) {
    const offset = new THREE.Vector3().copy(direction).multiplyScalar(movement);
    boxBody = box3ExpandByMovement(boxBody, direction, movement);
    // 无人机 沿着负 z轴移动时，不需要修改 boxGun
    if (direction.equals(zAxis)) {
      boxGun.translate(offset);
    } else if (!direction.equals(zNegAxis)) {
      boxGun = box3ExpandByMovement(boxGun, direction, movement);
    }
  }
  return [boxGun, boxBody];
}

function getDirectionAndScalar(srcPos, dstPos) {
  const dir = tmp.copy(dstPos).sub(srcPos);
  const scalar = dir.length();
  let maxIndex = 0;
  let maxDot = -Infinity;
  axes.forEach((axis, index) => {
    const dotVal = dir.dot(axis);
    if (maxDot < dotVal) {
      maxDot = dotVal;
      maxIndex = index;
    }
  });
  return [axes[maxIndex], scalar];
}

function findCollideBoxes(planeMat, srcPos, dstPos, blockingBoxes, areaBoxes) {
  const [direction, scalar] = getDirectionAndScalar(srcPos, dstPos);
  const [boxGun, boxBody] = boxArrFromMovementOfDrone(direction, scalar);
  return findCollideBoxesByDrone(planeMat, boxGun, boxBody, srcPos, blockingBoxes, areaBoxes);
}

function findCollideBoxesByDrone(planeMat, boxGun, boxBody, srcPos, blockingBoxes, areaBoxes) {
  boxGun.translate(srcPos);
  boxBody.translate(srcPos);
  const gunCollided = [];
  const bodyCollided = [];
  const areaCollided = [];
  blockingBoxes.forEach(box3 => {
    if (twoBoxesIntersected(box3, boxGun)) { // box3.intersectsBox(boxGun)
      gunCollided.push(box3);
    }
    if (twoBoxesIntersected(box3, boxBody)) {
      bodyCollided.push(box3);
    }
  });
  if (areaBoxes) {
    areaBoxes.forEach(box3 => {
      if (twoBoxesIntersected(box3, boxGun) || twoBoxesIntersected(box3, boxBody)) {
        areaCollided.push(box3);
      }
    });
  }
  const collidedCount = gunCollided.length + bodyCollided.length + areaCollided.length;
  return {gunCollided, bodyCollided, areaCollided, collidedCount, boxGun, boxBody};
}

/**
 * 无人机的喷嘴从 srcPos 移动到 dstPos， 无人机会碰撞到的所有障碍物的obb包围盒
 * @param {THREE.Matrix4} planeMat srcPos 和 dstPos 乘以此矩阵 得到世界坐标系坐标
 * @param {THREE.Vector3} srcPos 工作区本地坐标系坐标，喷嘴起始点
 * @param {THREE.Vector3} dstPos 工作区本地坐标系坐标，喷嘴目标点
 * @param {OBB[]} blockingObbs 所有障碍物的obb包围盒，位于世界坐标系
 * @return {{gunCollided: OBB[], bodyCollided: OBB[], collidedCount: number}} 无人机会碰撞到的所有障碍物的obb包围盒
 */
function findCollideObbs(planeMat, srcPos, dstPos, blockingObbs) {
  const [direction, scalar] = getDirectionAndScalar(srcPos, dstPos);
  const [boxGun, boxBody] = boxArrFromMovementOfDrone(direction, scalar);
  return findCollideObbsByBoxes(planeMat, boxGun, boxBody, srcPos, blockingObbs);
}

function findCollideObbsByBoxes(planeMat, boxGun, boxBody, srcPos, blockingObbs) {
  boxGun.translate(srcPos);
  boxBody.translate(srcPos);
  const obbGun = obbFromMatrixAndBox(planeMat, boxGun);
  const obbBody = obbFromMatrixAndBox(planeMat, boxBody);
  const gunCollided = [];
  const bodyCollided = [];
  blockingObbs.forEach(obb => {
    if (twoObbsIntersected(obb, obbGun)) { // obb.intersectsOBB(obbGun)
      gunCollided.push(obb);
    }
    if (twoObbsIntersected(obb, obbBody)) {
      bodyCollided.push(obb);
    }
  });
  const collidedCount = gunCollided.length + bodyCollided.length;

  return {gunCollided, bodyCollided, collidedCount, boxGun, boxBody, areaCollided: []};
}

function _3dTo2dPoint(direction, pt) {
  if (direction.equals(xAxis) || direction.equals(xNegAxis)) {
    return new THREE.Vector2(pt.z, pt.y); // {x: pt.z, y: pt.y}
  } else if (direction.equals(yAxis) || direction.equals(yNegAxis)) {
    return new THREE.Vector2(pt.x, pt.z);
  }
  return new THREE.Vector2(pt.x, pt.y);
}
function _2dTo3dPoint(direction, pt) {
  if (direction.equals(xAxis) || direction.equals(xNegAxis)) {
    return new THREE.Vector3(0, pt.y, pt.x);
  } else if (direction.equals(yAxis) || direction.equals(yNegAxis)) {
    return new THREE.Vector3(pt.x, 0, pt.y);
  }
  return new THREE.Vector3(pt.x, pt.y, 0);
}
function pointToId(point) {
  return `${point.x.toFixed(1)}, ${point.y.toFixed(1)}, ${point.z.toFixed(1)}`
}

/**
 * 计算出 沿direction方向的最多移动多少距离，刚好接触到障碍物
 * @param {PathCompute} ctx
 * @param {OBB[]} gunCollided
 * @param {OBB[]} bodyCollided
 * @param {THREE.Box3} boxGun
 * @param {THREE.Box3} boxBody
 * @param {THREE.Vector3} direction 本地坐标系，沿该方向上移动会撞击到障碍物
 * @param {THREE.Vector3} srcPos 喷嘴坐标，本地坐标系，无人机喷口点沿 正Z 轴移动 config.gapGunFront后的坐标
 * @return {number} 返回无人机 最多还能往direction 飞行多少距离。超过这个距离时无人机会和障碍物碰撞；为0，表示不能朝 direction方向飞行
 */
function computeForwardDistance(ctx, gunCollided, bodyCollided, boxGun, boxBody, direction, srcPos) {
  let dir = zAxis;
  if (direction.equals(zAxis) || direction.equals(zNegAxis)) {
    dir = xAxis;
  }
  const projectedBoxes = onlyBox3 ? computeBypassBoxSimple(gunCollided, bodyCollided, boxGun, boxBody, dir, false)
    : computeBypassBox(ctx, gunCollided, bodyCollided, boxGun, boxBody, dir, false);
  const field = axisToField.get(direction);
  const val = srcPos[field];
  let dist = Infinity;
  projectedBoxes.forEach(box3 => {
    const bMin = box3.min[field];
    const bMax = box3.max[field];
    if (isBetween(val, bMin, bMax)) { // val 不应该在 bMax 和 bMin之间；只能大于等于bMax，或小于等于bMin
      return;
    }
    const distance = Math.min(Math.abs(bMax - val), Math.abs(bMin - val));
    if (dist > distance) {
      dist = distance;
    }
  });
  if (dist === Infinity) {
    return 0;
  }
  return dist;
}

const _box = new THREE.Box3();
function computeBypassBoxSimple(gunCollided, bodyCollided, boxGun, boxBody, direction, needUnion = true) {
  const wholeBox = new THREE.Box3();
  const target = [];
  if (needUnion) {
    target.push(wholeBox);
  }
  const expandGun = getExpandVector(direction, false);
  const expandBody = getExpandVector(direction, true);
  gunCollided.forEach(box => {
    _box.copy(box).expandByVector(expandGun);
    if (needUnion) {
      wholeBox.union(_box);
    } else {
      target.push(_box.clone());
    }
  });
  bodyCollided.forEach(box => {
    _box.copy(box).expandByVector(expandBody);
    _box.translate(offsetY);
    if (needUnion) {
      wholeBox.union(_box);
    } else {
      target.push(_box.clone());
    }
  });
  return target;
}
/**
 * 计算boxGun 和 boxBody两个活动体积和 多个obb相重合的形状，在以direction为法线的平面上的投影包围盒
 * @param {PathCompute} ctx
 * @param {OBB[]} gunCollided 和枪身相撞的obb
 * @param {OBB[]} bodyCollided 和机身相撞的obb
 * @param {THREE.Box3} boxGun 枪身活动体积
 * @param {THREE.Box3} boxBody 机身活动体积
 * @param {THREE.Vector3} direction 本地坐标系，沿该方向上移动会撞击到障碍物
 */
function computeBypassBox(ctx, gunCollided, bodyCollided, boxGun, boxBody, direction, needUnion = true) {
  const {planeMatInv, blockingObbArr, blockingArr} = ctx
  const target = [];
  const wholeBox = new THREE.Box3();
  if (needUnion) {
    target.push(wholeBox);
  }
  gunCollided.forEach(obb => {
    const blocking = blockingArr[blockingObbArr.indexOf(obb)];
    const segments = segmentsFromBlocking(blocking, planeMatInv);
    const shape = createNoGpsShape(boxGun, segments, false, direction);
    const points = [];
    shape.forEach(pt => {
      const point = _2dTo3dPoint(direction, pt);
      wholeBox.expandByPoint(point);
      points.push(point);
    });
    if (!needUnion) {
      target.push(new THREE.Box3().setFromPoints(points));
    }
  });
  bodyCollided.forEach(obb => {
    const blocking = blockingArr[blockingObbArr.indexOf(obb)];
    const segments = segmentsFromBlocking(blocking, planeMatInv);
    const shape = createNoGpsShape(boxBody, segments, true, direction);
    const points = [];
    shape.forEach(pt => {
      const point = _2dTo3dPoint(direction, pt);
      wholeBox.expandByPoint(point);
      points.push(point);
    });
    if (!needUnion) {
      target.push(new THREE.Box3().setFromPoints(points));
    }
  });
  return target;
}

/**
 * 返回沿其余三个方向分别移动多少距离，可以避开障碍物
 * @param {THREE.Box3} projectedBox 障碍物投影到和direction垂直的平面，产生的二维包围盒
 * @param {THREE.Vector3} srcPos 喷嘴坐标，本地坐标系，无人机喷口点沿 正Z 轴移动 config.gapGunFront后的坐标
 * @param {THREE.Vector3} direction 六个轴之一，本地坐标系，沿该方向上移动会撞击到障碍物
 */
function computeBypassDistance(projectedBox, srcPos, direction) {
  const {min, max} = projectedBox;
  const droneCenter = new THREE.Vector3(srcPos.x, srcPos.y, srcPos.z - config.wholeCollideDeep * 0.5);
  const target = new Map();
  if (direction.equals(xAxis) || direction.equals(xNegAxis)) {
    target.set(zAxis, max.z - droneCenter.z);
    target.set(zNegAxis, droneCenter.z - min.z);
    target.set(yAxis, max.y - droneCenter.y);
    target.set(yNegAxis, droneCenter.y - min.y);
  } else if (direction.equals(yAxis) || direction.equals(yNegAxis)) {
    target.set(zAxis, max.z - droneCenter.z);
    target.set(zNegAxis, droneCenter.z - min.z);
    target.set(xAxis, max.x - droneCenter.x);
    target.set(xNegAxis, droneCenter.x - min.x);
  } else {
    target.set(yAxis, max.y - droneCenter.y);
    target.set(yNegAxis, droneCenter.y - min.y);
    target.set(xAxis, max.x - droneCenter.x);
    target.set(xNegAxis, droneCenter.x - min.x);
  }
  return target;
}

/**
 * @param {THREE.Matrix4} planeMat
 * @param {THREE.Box3} box
 * @return {OBB}
 */
function obbFromMatrixAndBox(planeMat, box) {
  const center = box.getCenter(new THREE.Vector3());
  const size = box.getSize(new THREE.Vector3());
  const obb = new OBB();
  obb.center.copy(center).applyMatrix4(planeMat);
  obb.halfSize.z = size.z * 0.5;
  obb.halfSize.y = size.y * 0.5;
  obb.halfSize.x = size.x * 0.5;
  obb.rotation.setFromMatrix4(planeMat);
  return obb;
}

/**
 * 返回长方体的十二条棱，注意：返回的十二条棱的 顶点位于 工作区的本地坐标系
 * @param {Blocking} blocking
 */
function segmentsFromBlocking(blocking, planeMatInv) {
  const eightPoints = blocking.getCorners(corners);
  if (planeMatInv) {
    eightPoints.forEach(it => it.applyMatrix4(planeMatInv));
  }
  return segmentsFromCorners(eightPoints);
}

/**
 * @param {THREE.Box3} box 工作区喷枪活动体积 或 工作区机身 活动体积
 * @param {THREE.Line3[]} segments, 长方体的十二条线段，位于工作区的本地坐标系
 * @param {boolean} bodyBox, true表示为无人机机身的活动空间
 * @param {THREE.Vector3} direction 沿哪个轴进行投影
 * @return {{x: number, y: number}[]} 返回无人机上的 gps 的活动边界，gps点不应该运动到此 凸包轮廓的内部
 */
function createNoGpsShape(box, segments, bodyBox = false, direction = zAxis) {
  const points = [];
  if (direction.equals(xAxis) || direction.equals(xNegAxis)) {
    planeDown.normal.copy(xAxis);
    planeUp.normal.copy(xAxis);
    planeDown.constant = -box.max.x;
    planeUp.constant = -box.min.x;
  } else if (direction.equals(yAxis) || direction.equals(yNegAxis)) {
    planeDown.normal.copy(yAxis);
    planeUp.normal.copy(yAxis);
    planeDown.constant = -box.max.y;
    planeUp.constant = -box.min.y;
  } else {
    planeDown.normal.copy(zAxis);
    planeUp.normal.copy(zAxis);
    planeDown.constant = -box.max.z;
    planeUp.constant = -box.min.z;
  }

  const set = new Set();
  segments.forEach(line3 => {
    const t1 = planeDown.intersectLine(line3, tmp);
    if (t1) {
      points.push(t1.clone());
    }
    const t2 = planeUp.intersectLine(line3, tmp);
    if (t2) {
      points.push(t2.clone());
    }
    set.add(line3.start).add(line3.end);
  });
  const segPts = [...set];
  segPts.forEach(point => {
    if (direction.equals(xAxis) || direction.equals(xNegAxis)) {
      if (isBetween(point.x, box.min.x, box.max.x)) {
        points.push(point);
      }
    } else if (direction.equals(yAxis) || direction.equals(yNegAxis)) {
      if (isBetween(point.y, box.min.y, box.max.y)) {
        points.push(point);
      }
    } else {
      if (isBetween(point.z, box.min.z, box.max.z)) {
        points.push(point);
      }
    }
  });
  if (!points.length) {
    return [];
  }

  let hh = config.gunCollideRadius;
  let hw = config.gunCollideRadius;
  if (bodyBox) {
    hh = config.bodyCollideHeight * 0.5;
    hw = config.bodyCollideWidth * 0.5;
  }
  if (direction.equals(xAxis) || direction.equals(xNegAxis)) {
    if (bodyBox) {
      hw = config.wholeCollideDeep * 0.5;
    }
  } else if (direction.equals(yAxis) || direction.equals(yNegAxis)) {
    if (bodyBox) {
      hh = config.wholeCollideDeep * 0.5;
    }
  }
  const pts = points.map(it => _3dTo2dPoint(direction, it));
  const contour = findConvexHull(pts);

  const pointsExpand = [];
  contour.forEach(pt => {
    const v0 = new THREE.Vector2(pt.x - hw, pt.y - hh);
    const v1 = new THREE.Vector2(pt.x + hw, pt.y - hh);
    const v2 = new THREE.Vector2(pt.x + hw, pt.y + hh);
    const v3 = new THREE.Vector2(pt.x - hw, pt.y + hh);
    pointsExpand.push(v0, v1, v2, v3);
  });
  const contourExpand = findConvexHull(pointsExpand);
  if (bodyBox && (direction.equals(xAxis) || direction.equals(xNegAxis) || direction.equals(zAxis) || direction.equals(zNegAxis))) {
    contourExpand.forEach(it => it.y -= config.centerToGun);
  }
  return contourExpand;
}

const _expandGun = new THREE.Vector3();
const _expandBody = new THREE.Vector3();
const offsetY = new THREE.Vector3(0, -config.centerToGun, 0);
function getExpandVector(direction, isBody) {
  const field = axisToField.get(direction);
  if (isBody) {
    _expandBody.set(config.bodyCollideWidth * 0.5, config.bodyCollideHeight * 0.5, config.wholeCollideDeep * 0.5);
    _expandBody[field] = 0;
    return _expandBody;
  } else {
    _expandGun.set(config.gunCollideRadius, config.gunCollideRadius, config.gapGunFront);
    _expandGun[field] = 0;
    return _expandGun;
  }
}

class DroneSprayPath {
  /**
   * @type {THREE.Vector3[]}
   */
  path
  len = 0
  dirs = ""
  constructor(param) {
    this.path = param.path;
    this.len = param.len;
    this.dirs = param.dirs;
  }
}

export function retrievePath(node) {
  let len = 0;
  const path = [];
  const nodes = [];
  let cur = node;
  while (cur) {
    path.push(cur.curPos);
    nodes.push(cur);
    cur = cur.preNode;
  }
  path.reverse();
  nodes.reverse();

  const dirs = [];
  nodes.forEach((it) => {
    if (it.axis) {
      const span = it.curPos.distanceTo(it.preNode.curPos)
      len += span;
      dirs.push(it.axis + ' ' + span.toFixed(2));
    }
  });
  len = parseFloat(len.toFixed(2));
  return new DroneSprayPath({path, len, dirs: dirs.join(', ')});
}

class PathNode {
  level = 0;
  id = "";
  axis = undefined;
  /**
   * 父级节点
   * @type {PathNode|null}
   */
  preNode = null;
  curPos = new THREE.Vector3();

  constructor(position, previous, direction) {
    this.curPos.copy(position);
    this.id = pointToId(position);
    if (direction) {
      this.axis = axisToStr.get(direction);
    }
    if (previous) {
      this.preNode = previous;
      this.level = previous.level + 1;
    }
  }
}

export class CornerComputeResult {
  /**
   * @type {workArea}
   */
  originArea
  /**
   * 若干个喷涂死角区域 把原始的工作区分割了若干个 子喷涂区
   * @type {[{x: number, y: number}[], [] (孔索引数组)]}
   */
  sprayAreas = []
  /**
   * 若干个喷涂死角区域的轮廓
   * @type {[{x: number, y: number}[], [] (孔索引数组)]}
   */
  corners = []

  constructor(workArea) {
    this.originArea = workArea;
    if (workArea.userData && workArea.userData.corners) {
      this.corners = workArea.userData.corners;
      this.sprayAreas = workArea.userData.subAreas;
    }
  }
}

// BoolPolygonsUtil.setPrecision(3)

export default class PathCompute {
  /**
   * @type {OBB[]} 位于世界坐标系
   */
  blockingObbArr
  /**
   * @type {Blocking[]}
   */
  blockingArr
  /**
   * @type {WorkArea[]}
   */
  workAreaArr
  /**
   * @type {WorkArea}
   */
  usingArea
  /**
   * 保存各个障碍物对应的包围盒
   * @type {THREE.Box3[]} 位于工作区本地坐标系，寻路时，根据选中工作区来初始化
   */
  blockingBoxArr = []
  /**
   * 保存各个工作区对应的包围盒
   * @type {THREE.Box3[]} 位于工作区本地坐标系
   */
  areaBoxArr = []
  /**
   * @type {THREE.Matrix4}
   */
  planeMat
  /**
   * @type {THREE.Matrix4}
   */
  planeMatInv

  /**
   * @param {Blocking[]} blockingArr
   * @param {WorkArea[]} workAreas
   */
  constructor(blockingArr, workAreas) {
    this.blockingObbArr = blockingArr.map(it => obbFromBlocking(it));
    this.blockingArr = blockingArr;
    this.workAreaArr = workAreas;
  }

  /**
   * 根据障碍物 和 工作区，计算出喷涂死角区域 (转喷头区域)
   * @param {WorkArea} workArea
   * @return {CornerComputeResult}
   */
  findCornersAndSprayAreas(workArea, scene) {
    const [boxA, boxB] = boxArrFromWorkArea(workArea);
    const obbA = obbFromMatrixAndBox(workArea.planeMat, boxA); //new OBB().fromBox3(boxA).applyMatrix4(workArea.planeMat)
    const obbB = obbFromMatrixAndBox(workArea.planeMat, boxB); //new OBB().fromBox3(boxB).applyMatrix4(workArea.planeMat)
    // scene.add(obbB_indicator);
    // obbB_indicator.initFromObb(obbB);
    const obbLst = this.blockingObbArr;
    const arrObbA = obbLst.filter(it => obbA.intersectsOBB(it, 0));
    const arrObbB = obbLst.filter(it => obbB.intersectsOBB(it, 0));
    const target = new CornerComputeResult(workArea);
    if (arrObbA.length === 0 && arrObbB.length === 0) {
      // 没有障碍物会影响到 该工作区的无人机喷涂作业
      return target;
    }
    const arrBlockingA = arrObbA.map(it => this.blockingArr[this.blockingObbArr.indexOf(it)]);
    const arrBlockingB = arrObbB.map(it => this.blockingArr[this.blockingObbArr.indexOf(it)]);
    const noGpsShapeArrA = [];
    const noGpsShapeArrB = [];
    arrBlockingA.forEach(blocking => {
      const segments = segmentsFromBlocking(blocking, workArea.planeMatInv);
      const shape = createNoGpsShape(boxA, segments, false);
      if (shape.length) {
        noGpsShapeArrA.push(shape);
      }
    });
    arrBlockingB.forEach(blocking => {
      const segments = segmentsFromBlocking(blocking, workArea.planeMatInv);
      const shape = createNoGpsShape(boxB, segments, true);
      if (shape.length) {
        noGpsShapeArrB.push(shape);
      }
    });

    // noGpsShapeArr 中所有的坐标 都位于 工作区workArea 的本地坐标系
    // const noGpsShapeArr0 = BoolPolygonsUtil.mergeNoHolePolygons(noGpsShapeArrA.concat(noGpsShapeArrB));
    const noGpsShapeArr0 = ClipperTool.mergeNoHolePolygons(noGpsShapeArrA.concat(noGpsShapeArrB));
    const outerShape = workArea.getOuterShape()
    const outerWorkAreaShape = [outerShape, []];
    const noGpsShapeArr = [];
    noGpsShapeArr0.forEach(shape => {
      // 求两个二维图形 的交集
      // const andShapes = BoolPolygonsUtil.polygonAintersectB(shape, outerWorkAreaShape);
      const andShapes = ClipperTool.polygonAintersectB(shape, outerWorkAreaShape);
      noGpsShapeArr.push(...andShapes);
      target.corners.push(...andShapes);
    });
    const workAreaShape = workArea.verts;
    let subWorkAreas = [workAreaShape];
    noGpsShapeArr0.forEach(shape => {
      let clipped = [];
      subWorkAreas.forEach(workShape => {
        // 求两个二维图形 的差集
        // const subShapes = BoolPolygonsUtil.polygonAsubB(workShape, shape);
        const subShapes = ClipperTool.polygonAsubB(workShape, shape);
        clipped.push(...subShapes);
      });
      subWorkAreas = clipped;
    });
    target.sprayAreas = subWorkAreas;
    return target;
  }

  /**
   * 检验无人机能否放置在 pos参数指定的坐标
   * @param {THREE.Vector3} worldPos 世界坐标系坐标，喷嘴 在工作区中的位置
   * @return {boolean} false 表示无人机无法放置在该位置，否则会和其他障碍物相撞
   */
  validateDronePosition(worldPos) {
    const planeMat = this.usingArea.planeMat;
    const pos = tmp2.copy(worldPos).applyMatrix4(this.usingArea.planeMatInv);
    const [boxGun, boxBody] = boxArrFromMovementOfDrone(null, 0);
    let collideObj;
    if (onlyBox3) {
      collideObj = findCollideBoxesByDrone(planeMat, boxGun, boxBody, pos, this.blockingBoxArr);
    } else {
      collideObj = findCollideObbsByBoxes(planeMat, boxGun, boxBody, pos, this.blockingObbArr);
    }
    return collideObj.collidedCount === 0;
  }

  initBlockingBoxArr(workArea) {
    const usingArea = selectMaxZArea(this.workAreaArr, workArea);
    if (this.usingArea === usingArea) {
      return;
    }
    this.usingArea = usingArea;
    if (onlyBox3) {
      this.blockingBoxArr.length = 0;
      this.blockingArr.forEach(blocking => {
        blocking.getCorners(corners);
        corners.forEach(it => it.applyMatrix4(usingArea.planeMatInv));
        const box3 = new THREE.Box3().setFromPoints(corners);
        this.blockingBoxArr.push(box3);
      });
      this.areaBoxArr.length = 0;
      this.workAreaArr.forEach(workArea => {
        const points = workArea.getLocalContour();
        points.forEach(it => it.applyMatrix4(workArea.planeMat).applyMatrix4(usingArea.planeMatInv));
        const box = new THREE.Box3().setFromPoints(points);
        this.areaBoxArr.push(box);
      });
    }
  }

  /**
   * 不考虑斜线飞行的基础寻路算法
   * @param {WorkArea} area
   * @param {THREE.Vector3} srcWorldPos 世界坐标，无人机喷头的起始点
   * @param {THREE.Vector3} dstWorldPos 世界坐标，无人机喷头的目标点
   * @return {DroneSprayPath[]}
   */
  findDronePath(area, srcWorldPos, dstWorldPos) {
    if (!area) {
      console.log('没有指定工作区');
      return [];
    }
    this.initBlockingBoxArr(area);
    const workArea = this.usingArea;
    const planeMat = workArea.planeMat;
    const blockingObbs = this.blockingObbArr;
    const blockingBoxes = this.blockingBoxArr;
    const areaBoxes = this.areaBoxArr;
    const srcPos = srcWorldPos.clone().applyMatrix4(workArea.planeMatInv);
    const dstPos = dstWorldPos.clone().applyMatrix4(workArea.planeMatInv);
    if (!this.validateDronePosition(srcWorldPos)) {
      console.log('起始点无效，无人机放置在该位置会和其他障碍物相撞');
      return [];
    }
    if (!this.validateDronePosition(dstWorldPos)) {
      console.log('目标点无效，无人机放置在该位置会和其他障碍物相撞');
      return [];
    }
    this.planeMat = workArea.planeMat;
    this.planeMatInv = workArea.planeMatInv;

    const bfs = []; // broad first search，无人机可以到达的坐标，才能临时添加到bfs数组
    const traversedPosIds = [];
    const directionOrders = [];
    const dstId = pointToId(dstPos);
    const findedPath = [];
    console.log('destination id: ', dstId);

    const addNodeToBfs = (newPos, curNode, direction) => {
      if (newPos.z > epsilon) {
        // 避免撞墙，因为newPos已经在工作区背面了
        return;
      }
      const node = new PathNode(newPos, curNode, direction);
      if (node.id === dstId) {
        findedPath.push(node);
      } else {
        if (!traversedPosIds.includes(node.id)) {
          bfs.push(node);
          traversedPosIds.push(node.id);
          if (direction) {
            directionOrders.push(node.axis);
          }
        }
      }
    }
    addNodeToBfs(srcPos);

    const processMoveDirection = (moveVal, direction, curNode) => {
      const pos = curNode.curPos;
      const moveVector = direction.clone().multiplyScalar(moveVal);
      const nextPos = pos.clone().add(moveVector);
      const collideResult = onlyBox3 ? findCollideBoxes(planeMat, pos, nextPos, blockingBoxes, areaBoxes) : findCollideObbs(planeMat, pos, nextPos, blockingObbs);
      if (collideResult.areaCollided.length > 0) {
        // 无人机沿 direction 方向飞行 moveVal距离后和工作区相撞，也就是撞墙了
        return;
      }
      if (collideResult.collidedCount === 0) {
        addNodeToBfs(nextPos, curNode, direction);
      } else {
        const {gunCollided, bodyCollided, boxGun, boxBody} = collideResult;
        const forwardMax = computeForwardDistance(this, gunCollided, bodyCollided, boxGun, boxBody, direction, pos);
        if (forwardMax > epsilon) {
          const nextP = direction.clone().multiplyScalar(forwardMax).add(pos);
          // if (validateDronePosition(planeMat, nextP, this))
          addNodeToBfs(nextP, curNode, direction);
        }
        const bypassBoxes = onlyBox3 ? computeBypassBoxSimple(gunCollided, bodyCollided, boxGun, boxBody, direction)
          : computeBypassBox(this, gunCollided, bodyCollided, boxGun, boxBody, direction);
        const bypassOptions = computeBypassDistance(bypassBoxes[0], pos, direction);
        bypassOptions.forEach((value, bypassDirection) => {
          const midPos = bypassDirection.clone().multiplyScalar(value).add(pos);
          if (midPos.z > epsilon) {
            // 避免撞墙，因为midPos已经在工作区背面了
            return;
          }
          const collideObj = onlyBox3 ? findCollideBoxes(planeMat, pos, midPos, blockingBoxes, areaBoxes) : findCollideObbs(planeMat, pos, midPos, blockingObbs);
          if (collideResult.areaCollided.length > 0) {
            return;
          }
          // const midPos2 = moveVector.clone().add(midPos);
          // const collideObj2 = findCollideObbs(planeMat, midPos2, midPos, blockingObbs);
          // if (collideObj2.collidedCount > 0) {
          //   findCollideObbs(planeMat, midPos, midPos2, blockingObbs);
          // }
          if (collideObj.collidedCount === 0) {
            addNodeToBfs(midPos, curNode, bypassDirection);
          } else {
            const {gunCollided, bodyCollided, boxGun, boxBody} = collideObj;
            const moveMax = computeForwardDistance(this, gunCollided, bodyCollided, boxGun, boxBody, bypassDirection, pos);
            if (moveMax <= epsilon) {
              return;
            }
            const nextP = bypassDirection.clone().multiplyScalar(moveMax).add(pos);
            // if (validateDronePosition(planeMat, nextP, this))
            addNodeToBfs(nextP, curNode, bypassDirection);
            // value 大于等于 moveMax, 表示为了绕开direction方向的障碍物，已经在 bypassDirection方向 的移动过程中，撞到了其他障碍物
            if (moveMax - value > epsilon) {
              // if (validateDronePosition(planeMat, nextP, this))
              addNodeToBfs(midPos, curNode, bypassDirection);
            }
          }
        });
      }
    };

    while (bfs.length > 0) {
      const curNode = bfs.shift();
      if (bfs.length > 5000) {
        console.log('分支节点过多')
        break;
      }
      const pos = curNode.curPos;
      const dx = Math.abs(dstPos.x - pos.x);
      const dy = Math.abs(dstPos.y - pos.y);
      const dz = Math.abs(dstPos.z - pos.z);

      const directions = [];
      const moveVals = [];
      if (dx > epsilon) {
        const diff = new THREE.Vector3(dstPos.x - pos.x, 0, 0);
        const direction = diff.x > 0 ? xAxis : xNegAxis;
        directions.push(direction);
        moveVals.push(Math.abs(dstPos.x - pos.x));
      }
      if (dy > epsilon) {
        const diff = new THREE.Vector3(0, dstPos.y - pos.y, 0);
        const direction = diff.y > 0 ? yAxis : yNegAxis;
        directions.push(direction);
        moveVals.push(Math.abs(dstPos.y - pos.y));
      }
      if (dz > epsilon) {
        const diff = new THREE.Vector3(0, 0, dstPos.z - pos.z);
        const direction = diff.z > 0 ? zAxis : zNegAxis;
        directions.push(direction);
        moveVals.push(Math.abs(dstPos.z - pos.z));
      }
      directions.forEach((diff, index) => {
        processMoveDirection(moveVals[index], directions[index], curNode);
      });

      if (findedPath.length > 17) {
        break
      }
    } // end while
    const pathArr = findedPath.map(it => retrievePath(it));
    console.log("pathArr: ", pathArr);
    return pathArr;
  }
}
